#include <ros/ros.h>
#include <vector>
#include "Eigen/Dense"
#include <cmath>
#include <stack>
#include <algorithm>
#include <sstream>
#include <fstream>
#include <iostream>

// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Path.h>

// opencv
#include <opencv-3.3.1-dev/opencv2/core.hpp>
#include <opencv-3.3.1-dev/opencv2/highgui.hpp>
#include <opencv-3.3.1-dev/opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//添加
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/radius_outlier_removal.h>

#include "Cplanning.hpp"

typedef Eigen::Matrix<double, 4, 4> Matrix4d;
typedef Eigen::Matrix<double, 4, 1> Vector4d;
typedef pcl::PointXYZ PointT;  //定义pointT为PointXYZRGBA类
typedef pcl::PointCloud<PointT> PointCloud;


using namespace std;
using Eigen::Matrix4d;
using Eigen::Vector4d;
using Eigen::Vector3d;
using Eigen::Vector3i;

ros::Publisher cloud2_pub;//体素化的点云
ros::Publisher point_pub;//pointarray发布
ros::Publisher world_pub;//ob_marker发布
ros::Publisher valid_pub;
ros::Publisher path_pub_;

visualization_msgs::MarkerArray obstacles;

vector< vector<float> > xlist;
vector< vector<float> > ylist;
vector< vector<float> > zlist;

static int dy = 2;
static int dx = 100;//4m
static int dz_min = 200;
static int dz_max = 10000;//10m

vector<vector<int>> world(dz_max/20+1, vector<int>(2*dx+1, 0));
vector<vector<int>> car(8, vector<int>(6, 1));
vector<vector<int>> valid_world(world.size()-car.size()+1, vector<int>(world[0].size()-car[0].size()+1, 0));

Vector3i deproject_pixel_to_point(int m, int n, double depth)
{
    // assert(intrin->model != RS2_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image
    //assert(intrin->model != RS2_DISTORTION_BROWN_CONRADY); // Cannot deproject to an brown conrady model
    // float x = (pixel[0] - intrin->ppx) / intrin->fx;
    // float y = (pixel[1] - intrin->ppy) / intrin->fy;
    double x = (n - 317.05316162109375) / 611.0570068359375;
    double y = (m - 245.96697998046875) / 609.8401489257812;
    // double r2  = x*x + y*y;
    // float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
    // float ux = x*f + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
    // float uy = y*f + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
    // double f = 1;
    // double ux = x*f;
    // double uy = y*f;
    // x = ux;
    // y = uy;
    return {depth * x/100.0+0.5, depth * y/100.0+0.5, depth/100.0+0.5};
    // return {depth * x/1000.0, depth * y/1000.0, depth/1000.0};
}

//点云处理///////////////////////////////////////////////////////////////////////////////////////////////
void depth_callback(const sensor_msgs::ImageConstPtr& depth_msg)
{   
    cv_bridge::CvImagePtr depth_ptr;
    depth_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1); 
    cv::Mat depth = depth_ptr->image;
    // cout<<"rows: "<<depth.rows<<" cols: "<<depth.cols<<"    ";
    // PointCloud::Ptr cloud(new PointCloud);
    for (int m = 0; m < depth.rows; m++){//480
        for (int n = 0; n < depth.cols; n++)//640
        {
            ushort d = depth.ptr<ushort>(m)[n];
            //前后限制
            if (d<dz_min || d >dz_max)
            {
                continue;
            }
            Vector3i point_pos;
            point_pos = deproject_pixel_to_point(m, n, d);
            //point_pos[1]上下， point_pos[2]前后， point_pos[0]左右
            if( point_pos[1]<-dy || point_pos[1]>dy || point_pos[0]<-dx || point_pos[0]>dx){
                continue;
            }

            world[point_pos[2]][point_pos[0]+dx] = 1;

        }
    }

    //可行空间地图（卷积）
    for(int i=0;i<world.size()-car.size()+1;i++){
        for(int j=0;j<world[0].size()-car[0].size()+1;j++){
            int sum = 0;
            for(int k=0;k<car.size();k++){
                for(int m=0;m<car[0].size();m++){
                    sum += world[i+k][j+m]*car[k][m];
                }
            }
            valid_world[i][j] = sum;
        }
    }

    //有效地图
    visualization_msgs::MarkerArray valid_map;
    valid_map.markers.resize(valid_world.size()*valid_world[0].size());
    int value_index = 0;
    for(int j=0;j<cp.valid_world.size();j++){
        for(int k=0;k<cp.valid_world[0].size();k++){
            visualization_msgs::Marker obstacle_point;
            obstacle_point.header.frame_id = "/camera_depth_optical_frame";
            obstacle_point.ns = "validworld";
            obstacle_point.pose.orientation.w = 1.0;
            obstacle_point.action = visualization_msgs::Marker::ADD;
            obstacle_point.type = visualization_msgs::Marker::CUBE;
            obstacle_point.id = value_index;
            obstacle_point.scale.x = 0.100;
            obstacle_point.scale.y = 0.100;
            obstacle_point.scale.z = 0.100;
            obstacle_point.color.r = 1.0-valid_world[j][k]/48.0;
            obstacle_point.color.g = 0;
            obstacle_point.color.b = 0;
            obstacle_point.color.a = valid_world[j][k]/48.0;
            obstacle_point.pose.position.x = k/10.0-4.8;
            obstacle_point.pose.position.y = 0;
            obstacle_point.pose.position.z = j/10.0+0.3;
            obstacle_point.header.stamp = ros::Time::now();
            valid_map.markers[value_index] = obstacle_point;
            value_index++;
            // cout<<cp.valid_world[j][k];
            // cp.valid_world[j][k] = 0;
        }
        // cout<<endl;
    }
    // cout<<"---------------------------"<<endl;
    // cout<<endl;
    // cout<<endl;
    valid_pub.publish(valid_map);

    
    //障碍物地图
    visualization_msgs::MarkerArray world_map;
    world_map.markers.resize(world.size()*world[0].size());
    value_index = 0;
    for(int j=0;j<world.size();j++){
        for(int k=0;k<world[0].size();k++){
            visualization_msgs::Marker obstacle_point;
            obstacle_point.header.frame_id = "/camera_depth_optical_frame";
            obstacle_point.ns = "obsworld";
            obstacle_point.pose.orientation.w = 1.0;
            obstacle_point.action = visualization_msgs::Marker::ADD;
            obstacle_point.type = visualization_msgs::Marker::CUBE;
            obstacle_point.id = value_index;
            obstacle_point.scale.x = 0.100;
            obstacle_point.scale.y = 0.100;
            obstacle_point.scale.z = 0.100;
            obstacle_point.color.r = 1.0-world[j][k];
            obstacle_point.color.g = 0;
            obstacle_point.color.b = 0;
            obstacle_point.color.a = world[j][k];
            obstacle_point.pose.position.x = k/10.0-5;
            obstacle_point.pose.position.y = 0;
            obstacle_point.pose.position.z = j/10.0;
            obstacle_point.header.stamp = ros::Time::now();
            world_map.markers[value_index] = obstacle_point;
            value_index++;
            // cout<<world[j][k];
            world[j][k] = 0;
        }
        // cout<<endl;
    }
    // cout<<"---------------------------"<<endl;
    // cout<<endl;
    // cout<<endl;
    world_pub.publish(world_map);

    cp.motion_plan();

    nav_msgs::Path gui_path;
    gui_path.poses.resize(cp.path_.size());
    gui_path.header.frame_id = "/camera_depth_optical_frame";
    int index = 0;
    for(auto iter = cp.path_.begin(); iter!=cp.path_.end(); ++iter){
        geometry_msgs::PoseStamped pose;
        pose.header.frame_id = "/camera_depth_optical_frame";
        pose.pose.position.x = iter->x_/10.0-4.8;
        pose.pose.position.y = -0.5;
        pose.pose.position.z = iter->z_/10.0+0.3;
        pose.pose.orientation.w = 1;
        gui_path.poses[index++] = pose;
    }
    gui_path.header.stamp = ros::Time::now();
    path_pub_.publish(gui_path);



}
 
int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "Cspace_planning");
  ros::NodeHandle nh;

  //获取pointcloud2并处理
  ros::Subscriber depth_sub = nh.subscribe<sensor_msgs::Image> ("/camera/depth/image_rect_raw", 1, depth_callback);

  // Create a ROS publisher for the output point cloud
//   cloud2_pub = nh.advertise<sensor_msgs::PointCloud2> ("/deal_pointcloud2", 1);
  world_pub = nh.advertise<visualization_msgs::MarkerArray>("/world", 1);
  valid_pub = nh.advertise<visualization_msgs::MarkerArray>("/valid", 1);
  path_pub_ = nh.advertise<nav_msgs::Path>("/path", 1);

  ros::Rate loop_rate(3);
  while(ros::ok()){
    ros::spinOnce();
    loop_rate.sleep();
    // usleep(1000*100);
  }


  return 0;
//   while(ros::ok()){
//     cloud2_pub.publish (cloud_vog);
//   }
}


